#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <errno.h>
#include <string.h>
#include "android_app_communication/androidLogMsg.h"

using namespace std;

#define FALSE -1
#define TRUE   0

int fd;

char powerOn[6] = {0x55, 0x04, 0x80, 0x00, 0xd1, 0xff};
char powerOff[6] = {0x55, 0x04, 0x81, 0x00, 0xd0, 0xff};
char lightLevel_1[6] = {0x55, 0x04, 0x82, 0x00, 0xd3, 0xff};
char lightLevel_2[6] = {0x55, 0x04, 0x82, 0x01, 0xd2, 0xff};
char lightLevel_3[6] = {0x55, 0x04, 0x82, 0x02, 0xd1, 0xff};
char lightLevel_4[6] = {0x55, 0x04, 0x82, 0x03, 0xd0, 0xff};
char lightLevel_5[6] = {0x55, 0x04, 0x82, 0x04, 0xd7, 0xff};
char lightLevel_6[6] = {0x55, 0x04, 0x82, 0x05, 0xd6, 0xff};
char desk_upView[6]   = {0x55, 0x04, 0x8e, 0x00, 0xdf, 0xff};
char desk_downView[6] = {0x55, 0x04, 0x8f, 0x00, 0xde, 0xff};
char back_upView[6]   = {0x55, 0x04, 0x90, 0x00, 0xc1, 0xff};
char back_downView[6] = {0x55, 0x04, 0x91, 0x00, 0xc0, 0xff};

ros::Publisher androidLog_pub;


void pubLogToAndroid(string log)
{
    android_app_communication::androidLogMsg msg;
    msg.message = log;
    androidLog_pub.publish(msg);
}

int UART_Open(int fd, const char* port)
{
    fd = open(port, O_RDWR|O_NOCTTY|O_NDELAY);
    if(FALSE == fd)
    {
        perror("Can't open projector serial port");
        return (FALSE);
    }
    if(fcntl(fd, F_SETFL, 0) < 0)
    {
        printf("fcntl failed!\n");
        return(FALSE);
    }
    if(0 == isatty(STDIN_FILENO))
    {
        printf("standard input is not a terminal device");
        return (FALSE);
    }
    else
    {
        printf("projector uart open successfully!");
    }

    return fd;
}

void UART_Close(int fd)
{
    close(fd);
}

int UART_Set(int fd, int speed, int flow_ctl,int databit,int stopbit,int parity)
{
    int i;
    int status;
    int speed_arr[] = {B115200, B19200, B9600, B4800, B2400, B1200};
    int name_arr[] = {115200, 19200, 9600, 4800, 2400, 1200};
    struct termios options;
    if(tcgetattr(fd, &options) != 0)
    {
        perror("setupSerial 1");
        return(FALSE);
    }
    for(i=0; i<sizeof(speed_arr)/sizeof(int); i++)
    {
        if(speed == name_arr[i])
        {
            cfsetispeed(&options, speed_arr[i]);
            cfsetospeed(&options, speed_arr[i]);
        }
    }
    options.c_cflag |= CLOCAL;
    options.c_cflag |= CREAD;

    switch(flow_ctl)
    {
        case 0:
            options.c_cflag &= ~CRTSCTS;
            break;
        case 1:
            options.c_cflag |= CRTSCTS;
            break;
        case 2:
            options.c_cflag |= IXON|IXOFF|IXANY;
            break;
    }
    options.c_cflag &= ~CSIZE;
    switch(databit)
    {
        case 5:
            options.c_cflag |= CS5;
            break;
        case 6:
            options.c_cflag|=CS6;
            break;
        case 7:
            options.c_cflag|=CS7;
            break;
        case 8:
            options.c_cflag|=CS8;
            break;
        default:
            fprintf(stderr, "Unsupported data size\n");
            return (FALSE);
    }
    switch(parity)
    {
        case 'n':
        case 'N':
            options.c_cflag &= ~PARENB;
            options.c_iflag &= ~INPCK;
            break;
        case 'o':
        case 'O':
            options.c_cflag |= (PARODD|PARENB);
            options.c_iflag |= INPCK;
            break;
        case 'e':
        case 'E':
            options.c_cflag|=PARENB;
            options.c_cflag&=~PARODD;
            options.c_iflag|=INPCK;
            break;
        case 's':
        case 'S':
            options.c_cflag &= ~PARENB;
            options.c_cflag &= ~CSTOPB;
            break;
        default:
            fprintf(stderr, "Unsupported parity\n");
            return(FALSE);
    }
    switch(stopbit)
    {
        case 1:
            options.c_cflag &= ~CSTOPB;
            break;
        case 2:
            options.c_cflag |= ~CSTOPB;
            break;
        default:
            fprintf(stderr, "Unsupported stop bits\n");
            return (FALSE);
    }
    options.c_oflag &= ~OPOST;
    options.c_lflag &= ~(ICANON|ECHO|ECHOE|ISIG);

    options.c_cc[VTIME] = 1;
    options.c_cc[VMIN] = 1;

    tcflush(fd, TCIFLUSH);

    if(tcsetattr(fd, TCSANOW, &options) != 0)
    {
        perror("UART set error!");
        return (FALSE);
    }

    return (TRUE);
}

int UART_Init(int fd, int speed, int flow_ctl, int databit, int stopbit, int parity)
{
    int err;
    if(UART_Set(fd, 115200, 0, 8, 1, 'N') == FALSE)
    {
        return FALSE;
    }
    else
    {
        return TRUE;
    }
}

int UART_Send(int fd, char *send_buf, int data_len)
{
    int len = 0;
    len = write(fd, send_buf, data_len);
    if(len == data_len)
    {
        return len;
    }
    else
    {
        tcflush(fd, TCOFLUSH);
        return FALSE;
    }
}

void cmdCallback(const std_msgs::Int32::ConstPtr& msg)
{
    ROS_INFO("Get projector control cmd: %d", msg->data);
    switch(msg->data)
    {
        case 1:
            if(6 == UART_Send(fd, powerOn, 6))
            {
                printf("Send power on cmd ok!");
                pubLogToAndroid("发送打开投影仪指令!");
            }
            break;
        case 2:
            if(6 == UART_Send(fd, powerOff, 6))
            { 
                printf("Send power off cmd ok!"); 
                pubLogToAndroid("发送关闭投影仪指令!");
            }
            break;
        case 3:
            if(6 == UART_Send(fd, lightLevel_1, 6))
            { printf("Send light level 1 cmd ok!");
              pubLogToAndroid("调整投影仪亮度:1");
            }
            break;
        case 4:
            if(6 == UART_Send(fd, lightLevel_2, 6))
            { printf("Send light level 2 cmd ok!");
              pubLogToAndroid("调整投影仪亮度:2");
            }
            break;
        case 5:
            if(6 == UART_Send(fd, lightLevel_3, 6))
            { printf("Send light level 3 cmd ok!");
              pubLogToAndroid("调整投影仪亮度:3");
            }
            break;
        case 6:
            if(6 == UART_Send(fd, lightLevel_4, 6))
            { printf("Send light level 4 cmd ok!");
              pubLogToAndroid("调整投影仪亮度:4");
            }
            break;
        case 7:
            if(6 == UART_Send(fd, lightLevel_5, 6))
            { printf("Send light level 5 cmd ok!");
              pubLogToAndroid("调整投影仪亮度:5");
            }
            break;
        case 8:
            if(6 == UART_Send(fd, lightLevel_6, 6))
            { printf("Send light level 5 cmd ok!");
              pubLogToAndroid("调整投影仪亮度:6");
            }
            break;
        case 9:
            if(6 == UART_Send(fd, desk_upView, 6))
            { printf("Send desk up view cmd ok!");
              pubLogToAndroid("投影仪:桌面正投");
            }
            break;
        case 10:
            if(6 == UART_Send(fd, desk_downView, 6))
            { printf("Send desk down view cmd ok!");
              pubLogToAndroid("投影仪:桌面背投");
            }
            break;
        case 11:
            if(6 == UART_Send(fd, back_upView, 6))
            { printf("Send back up view cmd ok!"); 
              pubLogToAndroid("投影仪:吊装正投");
            }
            break;
        case 12:
            if(6 == UART_Send(fd, back_downView, 6))
            { printf("Send back down view cmd ok!");
              pubLogToAndroid("投影仪:吊装背投");
            }
            break;
        default:
            break;
    }
}

int main(int argc, char **argv)
{
    int err;
    string androidLogTopic;
    string deviceName;

    ros::init(argc, argv, "projector_node");
    ros::NodeHandle n;
    ros::param::get("~androidLog_topic", androidLogTopic);
    ros::param::get("~device_name", deviceName);

    ros::Subscriber sub = n.subscribe("projector_cmd", 1, cmdCallback);
    androidLog_pub = n.advertise<android_app_communication::androidLogMsg>(androidLogTopic, 5);

    fd = UART_Open(fd, deviceName.c_str());
    do
    {
        err = UART_Init(fd, 115200, 0, 8, 1, 'N');
        printf("init projector uart successfully!");
    }while(FALSE == err || FALSE == fd);

    ros::spin();

    UART_Close(fd); //close serial port
    return 0;
}

